var mraa = require("mraa")
var pwms = new mraa.I2c(0)
var pwm0 = new mraa.Pwm(18)
var pwm1 = new mraa.Pwm(19)
var pGood = new mraa.Gpio(0)
var pError = new mraa.Gpio(1)

// ==========================================================================
// low level control
function sleep(ms) {
  var start = new Date().getTime();
  while(new Date().getTime() < start + ms);
}

// ==========================================================================
// low level control
pwms.address(0x40)
pwms.writeReg(0, 0x10)
pwms.writeReg(0xFE, 110) // 61Hz, 16.37ms, ~4us per unit
pwms.writeReg(0, 0x0)

pwm0.period_ms(20)
pwm0.enable(true)
pwm1.period_ms(20)
pwm1.enable(true)

// pin = 0~17
// value = us
function writePwm(pin, value)
{
  if(pin < 16)
  {
    var r
    value = value / 4;
    r = pwms.writeReg(6+pin*4, 0);
    if(r != 0)
    {
      pGood.write(0)
      pError.dir(0)
      pError.write(1)
      throw "<** PANIC **> failed to write PWM to I2C device!!!"
    }
    pwms.writeReg(7+pin*4, 0);
    pwms.writeReg(8+pin*4, value & 0xFF);
    pwms.writeReg(9+pin*4, (value>>8) & 0xFF);
  }
  else if(pin == 16)
  {
    pwm0.pulsewidth_us(value)
  }
  else if(pin == 17)
  {
    pwm1.pulsewidth_us(value)
  }
}

// ==========================================================================
// middleware
//
//  coordinates:
//    x: >0=left, <0=right 
//    y: >0=front, <0=back
//    z: >0=height
//    origin = body center

// leg <=> pin mapping
var pinMap = [
  [2, 6, 4], // 1
  [0, 3, 1], // 2
  [17, 7, 5], // 3
  [8, 16, 10], // 4
  [12, 15, 14], // 5
  [13, 9, 11], // 6
]

// center posisition of each pin
var pinDef = [
  [1432, 1500, 1400],                                                        
  [1480, 1580, 1480],                                                        
  [1400, 1460, 1460],                                           
  [1480, 1520, 1400],                                                        
  [1480, 1440, 1500],                                                        
  [1440, 1560, 1500],
]

// x, y, z coordinates of nodes of each leg 
var legCenters = [
  [40.36, 70.36, 0],
  [55.25, 0, 0],
  [40.36, -70.36, 0],
  [-40.36, -70.36, 0],
  [-55.25, 0, 0],
  [-40.36, 70.36, 0],
];

var legAngles = [45, 0, 315, 225, 180, 135]

// default x, y, z coordinates of tips of each leg 
var legTipsDefault = [
  [95.52, 125.52, -62, 1],
  [133.25, 0, -62, 1],
  [95.52, -125.52, -62, 1],
  [-95.52, -125.52, -62, 1],
  [-133.25, 0, -62, 1],
  [-95.52, 125.52, -62, 1],
];

// "current" x, y, z coordinates of tips of each leg 
var legTipsLoc = [
  [0, 0, 0],
  [0, 0, 0],
  [0, 0, 0],
  [0, 0, 0],
  [0, 0, 0],
  [0, 0, 0],
];


// return angles 0, 1, 2 of specified "leg" for its tip to reach specified "location"
// return null if impossible
function loc2angles(leg, loc)
{
  const a = 42, b = 84, c = 36

  // shift origin to leg center
  x = loc[0] - legCenters[leg][0]
  y = loc[1] - legCenters[leg][1]
  z = loc[2] - legCenters[leg][2]

  a0 = (180/Math.PI)*Math.atan(y/x) - legAngles[leg]
  if(x<0)
    a0 += 180
  else if(y<0)
    a0 += 360
  if(a0 > 180)
    a0 -= 360

  xp = Math.sqrt(x*x+y*y)-c
  yp = z

  lr = Math.sqrt(xp*xp+yp*yp)
  ar = (180/Math.PI)*Math.asin(yp/lr)
  at = (180/Math.PI)*Math.acos((lr*lr + a*a - b*b)/(2*a*lr))
  az = (180/Math.PI)*Math.acos((lr*lr - a*a + b*b)/(2*b*lr))
  a1 = at + ar
  a2 = 180 - at - az

  if(0) {
    console.log("\nxyz = " + x + "," + y + "," + z)
    console.log("len of reach = " + lr)
    console.log("angle of reach = " + ar)
    console.log("angle1 = " + at)
    console.log("angle2 = " + az)
    console.log("result = " + a0 + "," + a1 + "," + a2)
  }

  return [a0, a1, a2]
}

// nonblockingly move tip of specified "leg" to specified "location"
// return true if ok
// return false if impossible
function moveLeg(leg, loc)
{
  a = loc2angles(leg, loc)

  if(a[0] <= 45 && a[0] >= -45 &&
      a[1] <= 90 && a[1] >= -90 &&
      a[2] <= 150 && a[2] >= 36)
  {
    writePwm(pinMap[leg][0], pinDef[leg][0] + a[0] * 500 / 45)
    writePwm(pinMap[leg][1], pinDef[leg][1] - a[1] * 500 / 45)
    writePwm(pinMap[leg][2], pinDef[leg][2] + (a[2]-90) * 500 / 45)  
    legTipsLoc[leg][0] = loc[0]
    legTipsLoc[leg][1] = loc[1]
    legTipsLoc[leg][2] = loc[2]
  }
  else
  {
    console.log("moveLeg failed: " + leg + "," + loc + "," + a)
    return false
  }

  return true;
}

// return current tip location (in x, y, z) of specified "location"
function getLegsLoc(leg)
{
  return legTipsLoc[leg].slice(0, 3)
}

// ==========================================================================
// high level control
//

// Forward/backward waving
var _danceTable1 = [
  [[0.00, -12.00, 0.00],[0.00, -13.72, -3.46],[0.00, -15.02, -6.76],[0.00, -15.82, -9.83],[0.00, -16.08, -12.59],[0.00, -15.75, -14.95],[0.00, -14.83, -16.84],[0.00, -13.31, -18.17],[0.00, -11.24, -18.87],[0.00, -8.69, -18.89],[0.00, -5.74, -18.16],[0.00, -2.53, -16.69],[0.00, 0.79, -14.47],[0.00, 4.06, -11.57],[0.00, 7.12, -8.09],[0.00, 9.81, -4.18],[0.00, 12.00, 0.00],[0.00, 13.60, 4.24],[0.00, 14.56, 8.32],[0.00, 14.87, 12.05],[0.00, 14.53, 15.24],[0.00, 13.62, 17.74],[0.00, 12.19, 19.47],[0.00, 10.34, 20.35],[0.00, 8.15, 20.40],[0.00, 5.71, 19.64],[0.00, 3.10, 18.14],[0.00, 0.40, 16.01],[0.00, -2.33, 13.35],[0.00, -5.01, 10.31],[0.00, -7.57, 6.99],[0.00, -9.92, 3.51],],
  [[0.00, -12.00, 0.00],[0.00, -13.66, 0.39],[0.00, -14.79, 0.78],[0.00, -15.34, 1.11],[0.00, -15.31, 1.32],[0.00, -14.68, 1.40],[0.00, -13.51, 1.32],[0.00, -11.83, 1.09],[0.00, -9.70, 0.76],[0.00, -7.20, 0.38],[0.00, -4.42, -0.01],[0.00, -1.46, -0.34],[0.00, 1.56, -0.56],[0.00, 4.54, -0.63],[0.00, 7.34, -0.55],[0.00, 9.86, -0.33],[0.00, 12.00, 0.00],[0.00, 13.66, 0.39],[0.00, 14.79, 0.78],[0.00, 15.34, 1.11],[0.00, 15.31, 1.32],[0.00, 14.68, 1.40],[0.00, 13.51, 1.32],[0.00, 11.83, 1.09],[0.00, 9.70, 0.76],[0.00, 7.20, 0.38],[0.00, 4.42, -0.01],[0.00, 1.46, -0.34],[0.00, -1.56, -0.56],[0.00, -4.54, -0.63],[0.00, -7.34, -0.55],[0.00, -9.86, -0.33],],
  [[0.00, -12.00, 0.00],[0.00, -13.60, 4.24],[0.00, -14.56, 8.32],[0.00, -14.87, 12.05],[0.00, -14.53, 15.24],[0.00, -13.62, 17.74],[0.00, -12.19, 19.47],[0.00, -10.34, 20.35],[0.00, -8.15, 20.40],[0.00, -5.71, 19.64],[0.00, -3.10, 18.14],[0.00, -0.40, 16.01],[0.00, 2.33, 13.35],[0.00, 5.01, 10.31],[0.00, 7.57, 6.99],[0.00, 9.92, 3.51],[0.00, 12.00, 0.00],[0.00, 13.72, -3.46],[0.00, 15.02, -6.76],[0.00, 15.82, -9.83],[0.00, 16.08, -12.59],[0.00, 15.75, -14.95],[0.00, 14.83, -16.84],[0.00, 13.31, -18.17],[0.00, 11.24, -18.87],[0.00, 8.69, -18.89],[0.00, 5.74, -18.16],[0.00, 2.53, -16.69],[0.00, -0.79, -14.47],[0.00, -4.06, -11.57],[0.00, -7.12, -8.09],[0.00, -9.81, -4.18],],
  [[0.00, -12.00, 0.00],[0.00, -13.60, 4.24],[0.00, -14.56, 8.32],[0.00, -14.87, 12.05],[0.00, -14.53, 15.24],[0.00, -13.62, 17.74],[0.00, -12.19, 19.47],[0.00, -10.34, 20.35],[0.00, -8.15, 20.40],[0.00, -5.71, 19.64],[0.00, -3.10, 18.14],[0.00, -0.40, 16.01],[0.00, 2.33, 13.35],[0.00, 5.01, 10.31],[0.00, 7.57, 6.99],[0.00, 9.92, 3.51],[0.00, 12.00, 0.00],[0.00, 13.72, -3.46],[0.00, 15.02, -6.76],[0.00, 15.82, -9.83],[0.00, 16.08, -12.59],[0.00, 15.75, -14.95],[0.00, 14.83, -16.84],[0.00, 13.31, -18.17],[0.00, 11.24, -18.87],[0.00, 8.69, -18.89],[0.00, 5.74, -18.16],[0.00, 2.53, -16.69],[0.00, -0.79, -14.47],[0.00, -4.06, -11.57],[0.00, -7.12, -8.09],[0.00, -9.81, -4.18],],
  [[0.00, -12.00, 0.00],[0.00, -13.66, 0.39],[0.00, -14.79, 0.78],[0.00, -15.34, 1.11],[0.00, -15.31, 1.32],[0.00, -14.68, 1.40],[0.00, -13.51, 1.32],[0.00, -11.83, 1.09],[0.00, -9.70, 0.76],[0.00, -7.20, 0.38],[0.00, -4.42, -0.01],[0.00, -1.46, -0.34],[0.00, 1.56, -0.56],[0.00, 4.54, -0.63],[0.00, 7.34, -0.55],[0.00, 9.86, -0.33],[0.00, 12.00, 0.00],[0.00, 13.66, 0.39],[0.00, 14.79, 0.78],[0.00, 15.34, 1.11],[0.00, 15.31, 1.32],[0.00, 14.68, 1.40],[0.00, 13.51, 1.32],[0.00, 11.83, 1.09],[0.00, 9.70, 0.76],[0.00, 7.20, 0.38],[0.00, 4.42, -0.01],[0.00, 1.46, -0.34],[0.00, -1.56, -0.56],[0.00, -4.54, -0.63],[0.00, -7.34, -0.55],[0.00, -9.86, -0.33],],
  [[0.00, -12.00, 0.00],[0.00, -13.72, -3.46],[0.00, -15.02, -6.76],[0.00, -15.82, -9.83],[0.00, -16.08, -12.59],[0.00, -15.75, -14.95],[0.00, -14.83, -16.84],[0.00, -13.31, -18.17],[0.00, -11.24, -18.87],[0.00, -8.69, -18.89],[0.00, -5.74, -18.16],[0.00, -2.53, -16.69],[0.00, 0.79, -14.47],[0.00, 4.06, -11.57],[0.00, 7.12, -8.09],[0.00, 9.81, -4.18],[0.00, 12.00, 0.00],[0.00, 13.60, 4.24],[0.00, 14.56, 8.32],[0.00, 14.87, 12.05],[0.00, 14.53, 15.24],[0.00, 13.62, 17.74],[0.00, 12.19, 19.47],[0.00, 10.34, 20.35],[0.00, 8.15, 20.40],[0.00, 5.71, 19.64],[0.00, 3.10, 18.14],[0.00, 0.40, 16.01],[0.00, -2.33, 13.35],[0.00, -5.01, 10.31],[0.00, -7.57, 6.99],[0.00, -9.92, 3.51],],
]

// Left/right waving
var _danceTable2 = [
  [[-12.00, 0.00, 0.00],[-9.91, 0.00, 2.60],[-7.51, 0.00, 5.18],[-4.90, 0.00, 7.69],[-2.15, 0.00, 10.03],[0.65, 0.00, 12.10],[3.42, 0.00, 13.80],[6.07, 0.00, 15.03],[8.52, 0.00, 15.71],[10.70, 0.00, 15.75],[12.51, 0.00, 15.13],[13.87, 0.00, 13.84],[14.72, 0.00, 11.91],[14.98, 0.00, 9.43],[14.62, 0.00, 6.52],[13.62, 0.00, 3.32],[12.00, 0.00, 0.00],[9.82, 0.00, -3.26],[7.17, 0.00, -6.29],[4.17, 0.00, -8.96],[0.97, 0.00, -11.15],[-2.28, 0.00, -12.78],[-5.43, 0.00, -13.83],[-8.33, 0.00, -14.28],[-10.87, 0.00, -14.18],[-12.96, 0.00, -13.56],[-14.51, 0.00, -12.50],[-15.50, 0.00, -11.04],[-15.89, 0.00, -9.27],[-15.71, 0.00, -7.22],[-14.96, 0.00, -4.96],[-13.71, 0.00, -2.54],],
  [[-12.00, 0.00, 0.00],[-9.93, 0.00, 3.75],[-7.58, 0.00, 7.45],[-5.04, 0.00, 10.98],[-2.38, 0.00, 14.21],[0.33, 0.00, 17.01],[3.02, 0.00, 19.26],[5.62, 0.00, 20.82],[8.06, 0.00, 21.61],[10.25, 0.00, 21.54],[12.11, 0.00, 20.59],[13.55, 0.00, 18.75],[14.48, 0.00, 16.09],[14.84, 0.00, 12.72],[14.55, 0.00, 8.78],[13.60, 0.00, 4.47],[12.00, 0.00, 0.00],[9.80, 0.00, -4.41],[7.10, 0.00, -8.56],[4.03, 0.00, -12.25],[0.74, 0.00, -15.33],[-2.60, 0.00, -17.69],[-5.82, 0.00, -19.28],[-8.78, 0.00, -20.07],[-11.34, 0.00, -20.08],[-13.41, 0.00, -19.35],[-14.91, 0.00, -17.95],[-15.82, 0.00, -15.96],[-16.13, 0.00, -13.45],[-15.85, 0.00, -10.51],[-15.03, 0.00, -7.23],[-13.73, 0.00, -3.69],],
  [[-12.00, 0.00, 0.00],[-9.91, 0.00, 2.60],[-7.51, 0.00, 5.18],[-4.90, 0.00, 7.69],[-2.15, 0.00, 10.03],[0.65, 0.00, 12.10],[3.42, 0.00, 13.80],[6.07, 0.00, 15.03],[8.52, 0.00, 15.71],[10.70, 0.00, 15.75],[12.51, 0.00, 15.13],[13.87, 0.00, 13.84],[14.72, 0.00, 11.91],[14.98, 0.00, 9.43],[14.62, 0.00, 6.52],[13.62, 0.00, 3.32],[12.00, 0.00, 0.00],[9.82, 0.00, -3.26],[7.17, 0.00, -6.29],[4.17, 0.00, -8.96],[0.97, 0.00, -11.15],[-2.28, 0.00, -12.78],[-5.43, 0.00, -13.83],[-8.33, 0.00, -14.28],[-10.87, 0.00, -14.18],[-12.96, 0.00, -13.56],[-14.51, 0.00, -12.50],[-15.50, 0.00, -11.04],[-15.89, 0.00, -9.27],[-15.71, 0.00, -7.22],[-14.96, 0.00, -4.96],[-13.71, 0.00, -2.54],],
  [[-12.00, 0.00, 0.00],[-9.82, 0.00, -3.26],[-7.17, 0.00, -6.29],[-4.17, 0.00, -8.96],[-0.97, 0.00, -11.15],[2.28, 0.00, -12.78],[5.43, 0.00, -13.83],[8.33, 0.00, -14.28],[10.87, 0.00, -14.18],[12.96, 0.00, -13.56],[14.51, 0.00, -12.50],[15.50, 0.00, -11.04],[15.89, 0.00, -9.27],[15.71, 0.00, -7.22],[14.96, 0.00, -4.96],[13.71, 0.00, -2.54],[12.00, 0.00, 0.00],[9.91, 0.00, 2.60],[7.51, 0.00, 5.18],[4.90, 0.00, 7.69],[2.15, 0.00, 10.03],[-0.65, 0.00, 12.10],[-3.42, 0.00, 13.80],[-6.07, 0.00, 15.03],[-8.52, 0.00, 15.71],[-10.70, 0.00, 15.75],[-12.51, 0.00, 15.13],[-13.87, 0.00, 13.84],[-14.72, 0.00, 11.91],[-14.98, 0.00, 9.43],[-14.62, 0.00, 6.52],[-13.62, 0.00, 3.32],],
  [[-12.00, 0.00, 0.00],[-9.80, 0.00, -4.41],[-7.10, 0.00, -8.56],[-4.03, 0.00, -12.25],[-0.74, 0.00, -15.33],[2.60, 0.00, -17.69],[5.82, 0.00, -19.28],[8.78, 0.00, -20.07],[11.34, 0.00, -20.08],[13.41, 0.00, -19.35],[14.91, 0.00, -17.95],[15.82, 0.00, -15.96],[16.13, 0.00, -13.45],[15.85, 0.00, -10.51],[15.03, 0.00, -7.23],[13.73, 0.00, -3.69],[12.00, 0.00, 0.00],[9.93, 0.00, 3.75],[7.58, 0.00, 7.45],[5.04, 0.00, 10.98],[2.38, 0.00, 14.21],[-0.33, 0.00, 17.01],[-3.02, 0.00, 19.26],[-5.62, 0.00, 20.82],[-8.06, 0.00, 21.61],[-10.25, 0.00, 21.54],[-12.11, 0.00, 20.59],[-13.55, 0.00, 18.75],[-14.48, 0.00, 16.09],[-14.84, 0.00, 12.72],[-14.55, 0.00, 8.78],[-13.60, 0.00, 4.47],],
  [[-12.00, 0.00, 0.00],[-9.82, 0.00, -3.26],[-7.17, 0.00, -6.29],[-4.17, 0.00, -8.96],[-0.97, 0.00, -11.15],[2.28, 0.00, -12.78],[5.43, 0.00, -13.83],[8.33, 0.00, -14.28],[10.87, 0.00, -14.18],[12.96, 0.00, -13.56],[14.51, 0.00, -12.50],[15.50, 0.00, -11.04],[15.89, 0.00, -9.27],[15.71, 0.00, -7.22],[14.96, 0.00, -4.96],[13.71, 0.00, -2.54],[12.00, 0.00, 0.00],[9.91, 0.00, 2.60],[7.51, 0.00, 5.18],[4.90, 0.00, 7.69],[2.15, 0.00, 10.03],[-0.65, 0.00, 12.10],[-3.42, 0.00, 13.80],[-6.07, 0.00, 15.03],[-8.52, 0.00, 15.71],[-10.70, 0.00, 15.75],[-12.51, 0.00, 15.13],[-13.87, 0.00, 13.84],[-14.72, 0.00, 11.91],[-14.98, 0.00, 9.43],[-14.62, 0.00, 6.52],[-13.62, 0.00, 3.32],],
]

// Rotate by Z axis
var _danceTable3 = [
  [[0.00, 8.86, 22.74],[-1.92, 9.24, 19.56],[-3.87, 9.35, 15.81],[-5.77, 9.12, 11.61],[-7.54, 8.52, 7.07],[-9.12, 7.52, 2.34],[-10.42, 6.12, -2.45],[-11.40, 4.36, -7.14],[-12.01, 2.29, -11.58],[-12.22, 0.00, -15.64],[-12.01, -2.40, -19.19],[-11.40, -4.80, -22.12],[-10.42, -7.07, -24.33],[-9.12, -9.09, -25.77],[-7.54, -10.76, -26.39],[-5.77, -11.98, -26.19],[-3.87, -12.72, -25.18],[-1.92, -12.94, -23.38],[0.00, -12.67, -20.85],[1.83, -11.95, -17.67],[3.53, -10.86, -13.93],[5.04, -9.48, -9.73],[6.34, -7.91, -5.19],[7.41, -6.24, -0.46],[8.25, -4.57, 4.33],[8.84, -2.94, 9.02],[9.20, -1.41, 13.47],[9.32, 0.00, 17.53],[9.20, 1.30, 21.08],[8.84, 2.50, 24.00],[8.25, 3.61, 26.21],[7.41, 4.66, 27.64],[6.34, 5.67, 28.27],[5.04, 6.62, 28.07],[3.53, 7.49, 27.06],[1.83, 8.25, 25.26],],
  [[0.00, 10.77, 0.94],[-1.94, 11.29, -3.04],[-3.94, 11.40, -6.90],[-5.91, 11.05, -10.54],[-7.78, 10.20, -13.84],[-9.45, 8.87, -16.71],[-10.85, 7.09, -19.05],[-11.91, 4.95, -20.78],[-12.57, 2.54, -21.84],[-12.79, 0.00, -22.20],[-12.57, -2.54, -21.84],[-11.91, -4.95, -20.78],[-10.85, -7.09, -19.05],[-9.45, -8.87, -16.71],[-7.78, -10.20, -13.84],[-5.91, -11.05, -10.54],[-3.94, -11.40, -6.90],[-1.94, -11.29, -3.04],[0.00, -10.77, 0.94],[1.82, -9.91, 4.92],[3.46, -8.81, 8.78],[4.90, -7.55, 12.42],[6.10, -6.22, 15.72],[7.08, -4.89, 18.59],[7.82, -3.59, 20.93],[8.33, -2.35, 22.66],[8.64, -1.16, 23.72],[8.74, -0.00, 24.08],[8.64, 1.16, 23.72],[8.33, 2.35, 22.66],[7.82, 3.59, 20.93],[7.08, 4.89, 18.59],[6.10, 6.22, 15.72],[4.90, 7.55, 12.42],[3.46, 8.81, 8.78],[1.82, 9.91, 4.92],],
  [[0.00, 12.67, -20.85],[-1.92, 12.94, -23.38],[-3.87, 12.72, -25.18],[-5.77, 11.98, -26.19],[-7.54, 10.76, -26.39],[-9.12, 9.09, -25.77],[-10.42, 7.07, -24.33],[-11.40, 4.80, -22.12],[-12.01, 2.40, -19.19],[-12.22, 0.00, -15.64],[-12.01, -2.29, -11.58],[-11.40, -4.36, -7.14],[-10.42, -6.12, -2.45],[-9.12, -7.52, 2.34],[-7.54, -8.52, 7.07],[-5.77, -9.12, 11.61],[-3.87, -9.35, 15.81],[-1.92, -9.24, 19.56],[0.00, -8.86, 22.74],[1.83, -8.25, 25.26],[3.53, -7.49, 27.06],[5.04, -6.62, 28.07],[6.34, -5.67, 28.27],[7.41, -4.66, 27.64],[8.25, -3.61, 26.21],[8.84, -2.50, 24.00],[9.20, -1.30, 21.08],[9.32, 0.00, 17.53],[9.20, 1.41, 13.47],[8.84, 2.94, 9.02],[8.25, 4.57, 4.33],[7.41, 6.24, -0.46],[6.34, 7.91, -5.19],[5.04, 9.48, -9.73],[3.53, 10.86, -13.93],[1.83, 11.95, -17.67],],
  [[0.00, 12.67, -20.85],[-1.83, 11.95, -17.67],[-3.53, 10.86, -13.93],[-5.04, 9.48, -9.73],[-6.34, 7.91, -5.19],[-7.41, 6.24, -0.46],[-8.25, 4.57, 4.33],[-8.84, 2.94, 9.02],[-9.20, 1.41, 13.47],[-9.32, 0.00, 17.53],[-9.20, -1.30, 21.08],[-8.84, -2.50, 24.00],[-8.25, -3.61, 26.21],[-7.41, -4.66, 27.64],[-6.34, -5.67, 28.27],[-5.04, -6.62, 28.07],[-3.53, -7.49, 27.06],[-1.83, -8.25, 25.26],[0.00, -8.86, 22.74],[1.92, -9.24, 19.56],[3.87, -9.35, 15.81],[5.77, -9.12, 11.61],[7.54, -8.52, 7.07],[9.12, -7.52, 2.34],[10.42, -6.12, -2.45],[11.40, -4.36, -7.14],[12.01, -2.29, -11.58],[12.22, 0.00, -15.64],[12.01, 2.40, -19.19],[11.40, 4.80, -22.12],[10.42, 7.07, -24.33],[9.12, 9.09, -25.77],[7.54, 10.76, -26.39],[5.77, 11.98, -26.19],[3.87, 12.72, -25.18],[1.92, 12.94, -23.38],],
  [[0.00, 10.77, 0.94],[-1.82, 9.91, 4.92],[-3.46, 8.81, 8.78],[-4.90, 7.55, 12.42],[-6.10, 6.22, 15.72],[-7.08, 4.89, 18.59],[-7.82, 3.59, 20.93],[-8.33, 2.35, 22.66],[-8.64, 1.16, 23.72],[-8.74, 0.00, 24.08],[-8.64, -1.16, 23.72],[-8.33, -2.35, 22.66],[-7.82, -3.59, 20.93],[-7.08, -4.89, 18.59],[-6.10, -6.22, 15.72],[-4.90, -7.55, 12.42],[-3.46, -8.81, 8.78],[-1.82, -9.91, 4.92],[0.00, -10.77, 0.94],[1.94, -11.29, -3.04],[3.94, -11.40, -6.90],[5.91, -11.05, -10.54],[7.78, -10.20, -13.84],[9.45, -8.87, -16.71],[10.85, -7.09, -19.05],[11.91, -4.95, -20.78],[12.57, -2.54, -21.84],[12.79, -0.00, -22.20],[12.57, 2.54, -21.84],[11.91, 4.95, -20.78],[10.85, 7.09, -19.05],[9.45, 8.87, -16.71],[7.78, 10.20, -13.84],[5.91, 11.05, -10.54],[3.94, 11.40, -6.90],[1.94, 11.29, -3.04],],
  [[0.00, 8.86, 22.74],[-1.83, 8.25, 25.26],[-3.53, 7.49, 27.06],[-5.04, 6.62, 28.07],[-6.34, 5.67, 28.27],[-7.41, 4.66, 27.64],[-8.25, 3.61, 26.21],[-8.84, 2.50, 24.00],[-9.20, 1.30, 21.08],[-9.32, 0.00, 17.53],[-9.20, -1.41, 13.47],[-8.84, -2.94, 9.02],[-8.25, -4.57, 4.33],[-7.41, -6.24, -0.46],[-6.34, -7.91, -5.19],[-5.04, -9.48, -9.73],[-3.53, -10.86, -13.93],[-1.83, -11.95, -17.67],[0.00, -12.67, -20.85],[1.92, -12.94, -23.38],[3.87, -12.72, -25.18],[5.77, -11.98, -26.19],[7.54, -10.76, -26.39],[9.12, -9.09, -25.77],[10.42, -7.07, -24.33],[11.40, -4.80, -22.12],[12.01, -2.40, -19.19],[12.22, 0.00, -15.64],[12.01, 2.29, -11.58],[11.40, 4.36, -7.14],[10.42, 6.12, -2.45],[9.12, 7.52, 2.34],[7.54, 8.52, 7.07],[5.77, 9.12, 11.61],[3.87, 9.35, 15.81],[1.92, 9.24, 19.56],],
]


function _dance(l, t, loc) {

  var N = stepData[0].length
  if(t >= N)
    t = 0
  var ref = stepData[l][t]

  loc[0] = ref[0]
  loc[1] = ref[1]
  loc[2] = ref[2]

  return (t+N+stepOrder)%(N)
}

var stepDelay = 20

var stepData
var stepOrder
var stepFunc = null

var currMode = null
var currStep = 0         // current time

var _switchTable = []

function _next() {

  if(_switchTable.length != 0)  // during switching
    return

  var nextStep;
  var loc = [0, 0, 0]

  for(var i=0;i<6;i++) 
  {
    nextStep = stepFunc(i, currStep, loc)

    loc[0] += legTipsDefault[i][0]
    loc[1] += legTipsDefault[i][1]
    loc[2] += legTipsDefault[i][2]

    if(0)
      console.log("Steps[" + currstep + "]Leg[" + (i+1) + "]" + loc[0] + "," + loc[1] + "," + loc[2])

    moveLeg(i, loc)

  }
  currStep = nextStep
  setTimeout(_next, stepDelay)
}

function _switching()
{
  if(_switchTable.length == 0)
  {
    // start dancing
    _next()
    return;
  }

  locs = _switchTable.pop()

  for(i=0;i<6;i++)
  {
    var loc = locs[i].slice()
    loc[0] += legTipsDefault[i][0]
    loc[1] += legTipsDefault[i][1]
    loc[2] += legTipsDefault[i][2]
    moveLeg(i, loc)
  }

  setTimeout(_switching, stepDelay)
}

var modeTable = [
  [_dance, _danceTable1, 1],
  [_dance, _danceTable1, -1],
  [_dance, _danceTable2, 1],
  [_dance, _danceTable2, -1],
  [_dance, _danceTable3, 1],
  [_dance, _danceTable3, -1],
]

function _switch() {

  // random choose next step

  var newMode = Math.floor(Math.random()*modeTable.length)%modeTable.length
  if(newMode == currMode)
  {
    setTimeout(_switch, 1000) // next switch event
    return
  }
  setTimeout(_switch, 5000) // next switch event

  currMode = newMode
  currStep = 0
  stepFunc = modeTable[currMode][0]
  stepData = modeTable[currMode][1]
  stepOrder = modeTable[currMode][2]

  // prepare for the path of switching
  const switchSteps = 5

  var i
  var starts = []
  var ends = []
  var steps = []
  for(i=0;i<6;i++)
  {
    var s = getLegsLoc(i)
    s[0] -= legTipsDefault[i][0]
    s[1] -= legTipsDefault[i][1]
    s[2] -= legTipsDefault[i][2]

    var e = [0, 0, 0]
    stepFunc(i, 0, e)

    var d = [(s[0]-e[0])/switchSteps, (s[1]-e[1])/switchSteps, (s[2]-e[2])/switchSteps]

    starts.push(s)
    ends.push(e)
    steps.push(d)
  }

  _switchTable.push(ends)
  for(var t=1;t<=switchSteps;t++)
  {
    pts = []
    for(i=0;i<6;i++)
    {
      pt = ends[i].slice()
      pt[0] += steps[i][0]*t
      pt[1] += steps[i][1]*t
      pt[2] += steps[i][2]*t
      pts.push(pt)
    }
    _switchTable.push(pts)
  }

  _switching()
}

function init()
{
  for(i=0;i<6;i++)
    moveLeg(i, legTipsDefault[i])
  sleep(100)
}

init()
console.log("go!")
pGood.dir(0)
pGood.write(1)

_switch()
